#include "main.h"
#include "motorctrltask.h"

#include "FreeRTOS.h"
#include "task.h"
#include "tim.h"
#include "math.h"
#include "MPU6050.h"
struct PID_Param Wheel_PID;
struct EncoderValue  EncoderValue;
struct Motor LMotor;
struct Motor RMotor;
void GetCoderValue()
{
		EncoderValue.LValue=(int16_t)__HAL_TIM_GetCounter(&htim4);
		EncoderValue.RValue=(int16_t)__HAL_TIM_GetCounter(&htim1);	 		
		__HAL_TIM_SetCounter(&htim1,0);
		__HAL_TIM_SetCounter(&htim4,0);
}

/**************************************************************************
函数功能：赋值给PWM寄存器
入口参数：左轮PWM、右轮PWM
返回  值：无
**************************************************************************/
void Set_Pwm(float PWMA,float PWMB)
{
	if(PWMA>0)	
	{
		if(PWMA>1) PWMA=1;
		HAL_GPIO_WritePin(AIN1_GPIO_Port,AIN1_Pin,GPIO_PIN_SET);
		HAL_GPIO_WritePin(AIN2_GPIO_Port,AIN2_Pin,GPIO_PIN_RESET);
	}
	else 	
	{
		if(PWMA<-1) PWMA=-1;
		HAL_GPIO_WritePin(AIN1_GPIO_Port,AIN1_Pin,GPIO_PIN_RESET);
		HAL_GPIO_WritePin(AIN2_GPIO_Port,AIN2_Pin,GPIO_PIN_SET);
	}    
	if(PWMB>0)	
	{
		if(PWMB>1) PWMB=1;
		HAL_GPIO_WritePin(BIN1_GPIO_Port,BIN1_Pin,GPIO_PIN_RESET);
		HAL_GPIO_WritePin(BIN2_GPIO_Port,BIN2_Pin,GPIO_PIN_SET);
	}
	else 	
	{
		if(PWMB<-1) PWMB=-1;
		HAL_GPIO_WritePin(BIN1_GPIO_Port,BIN1_Pin,GPIO_PIN_SET);
		HAL_GPIO_WritePin(BIN2_GPIO_Port,BIN2_Pin,GPIO_PIN_RESET);
	} 
        
	__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_1,(uint16_t)(fabs(PWMA)*4499));
	__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_4,(uint16_t)(fabs(PWMB)*4499));
}
/**************************************************************************
函数功能：异常关闭电机
入口参数：倾角和电压
返回  值：1：异常  0：正常
**************************************************************************/
uint8_t Turn_Off(float angle, int voltage)
{

		if(angle<-40||angle>40) //===倾角大于40度关闭电机
		{	     
				HAL_GPIO_WritePin(STBY_GPIO_Port,STBY_Pin,GPIO_PIN_RESET);
		}
		else
		{
				HAL_GPIO_WritePin(STBY_GPIO_Port,STBY_Pin,GPIO_PIN_SET);
		}
	
}
void MotorCtrl_Task(void* pvParameters)
{  
	
	static portTickType PreviousWakeTime; 
	PreviousWakeTime = xTaskGetTickCount();
	
	taskENTER_CRITICAL(); //进入临界区
	HAL_TIM_Encoder_Start(&htim1,TIM_CHANNEL_ALL);
	HAL_TIM_Encoder_Start(&htim4,TIM_CHANNEL_ALL);
	HAL_GPIO_WritePin(STBY_GPIO_Port,STBY_Pin,GPIO_PIN_SET);
	HAL_TIM_PWM_Start(&htim2,TIM_CHANNEL_1);
	HAL_TIM_PWM_Start(&htim2,TIM_CHANNEL_4);
	taskEXIT_CRITICAL(); //退出临界区
	while(1)
	{
		GetCoderValue();
		Set_Pwm(LMotor.duty,RMotor.duty);
		Turn_Off(Roll,0);
		vTaskDelayUntil(&PreviousWakeTime,pdMS_TO_TICKS(5));		//绝对延时函数 保证PID时间准确性
	}
}


